#ifndef __TEXTURE_H
#define __TEXTURE_H

#include <opencv/cv.h>
#include <pmmintrin.h>
#include <iostream>
#include <cmath>
#include <opencv/highgui.h>
#include <string>
#include <boost/lexical_cast.hpp>
#include "Scarf.h"

template <class T>
class KeyPoint
{
public:
  KeyPoint(int x, int y, int scale, T data)
  {
    this->x = x;
    this->y = y;
    this->scale = scale;
    this->data = data;
  }

  int x, y;
  int scale;
  T data;
  std::vector<float> vTexture;
};

template <class T>
class Texture
{
public:
  Texture(const CvSize imsize, const int sGraylevels);
  ~Texture();

  inline const int WindowSize() { return m_window; };
  inline const int Offset() { return m_offset; };
  inline const int GrayLevels() { return m_graylevels; };

  void convertRGBtoScaledHSV(const IplImage* rgb);
  const IplImage* GetHue() { return m_h; };
  const IplImage* GetSaturation() { return m_s; };
  const IplImage* GetValue() { return m_v; };

  const IplImage* convertRGBtoExG(const IplImage* rgb);
  const IplImage* convertRGBtoExR(const IplImage* rgb);
  const IplImage* convertRGBtoCIVE(const IplImage* rgb);

  void DescribeTextureCCM(const IplImage *src, typename std::vector<KeyPoint<T> >::iterator first, typename std::vector<KeyPoint<T> >::iterator last);
  void DescribeTextureSCARF(const IplImage *src, typename std::vector<KeyPoint<T> >::iterator first, typename std::vector<KeyPoint<T> >::iterator last);
  void DescribeTextureGabor(const IplImage *src, typename std::vector<KeyPoint<T> >::iterator first, typename std::vector<KeyPoint<T> >::iterator last);
  void DescribeColour(const IplImage *hue, const IplImage *saturation, const IplImage *value, typename std::vector<KeyPoint<T> >::iterator first, typename std::vector<KeyPoint<T> >::iterator last);
  void DescribeColour(const IplImage *rgb, typename std::vector<KeyPoint<T> >::iterator first, typename std::vector<KeyPoint<T> >::iterator last);
  void DescribeVegetation(const IplImage *src, typename std::vector<KeyPoint<T> >::iterator first, typename std::vector<KeyPoint<T> >::iterator last);

  inline const int DescriptorLengthCCM() { return 8; };
  inline const int DescriptorLengthSCARF() { return 60; };
  inline const int DescriptorLengthGabor() { return m_gabor_scales*m_gabor_orientations; };
  inline const int DescriptorLengthColour() { return 64; }; //TODO.. pick best
  inline const int DescriptorLengthVegetation() { return 16; };

private:
  const CvSize m_imsize;
  const int m_window;
  const int m_border;
  const int m_offset;
  const int m_graylevels;
  const double m_scale;
  const float m_prob, m_prob0;
  const int m_hueshift;
  const int m_valshift;
  const int m_threshold;
  IplImage *m_h;
  IplImage *m_s;
  IplImage *m_v;
  IplImage *m_tmp;
  IplImage *m_con;
  IplImage *m_idm;
  IplImage *m_sum;
  IplImage *m_dif;
  IplImage *m_comatrix;
  IplImage *m_comatrixt;
  IplImage *m_result;
  float *m_asum;
  float *m_adif;
  float* m_comatrix_data;
  int* m_sum_data;
  int* m_dif_data;
  int* m_huehist;
  int* m_valhist;
  int* m_colhist;

  const int m_gabor_scales;
  const int m_gabor_orientations;
  const double m_gabor_fmax;
  const double m_gabor_sigma;
  CvMat **m_gabor;

  static const int m_hsv_shift;
  static const int m_div_table255[256];
  int m_div_tableG[256];
  int m_map_tableG[361];

  Scarf *mScarf;

  void CoOccurenceMatrix(const IplImage *ref, const IplImage *cmp, const int x, const int y, const int d);
  void CoOccurenceMatrixSSE_D0(const IplImage *ref, const IplImage *cmp, const int x, const int y); // cooccurrence between pixels of ref & cmp without offset
  void CoOccurenceMatrixSSE_D1(const IplImage *ref, const IplImage *cmp, const int x, const int y);
  void CoOccurenceMatrixSSE_D2(const IplImage *ref, const IplImage *cmp, const int x, const int y);
  void CoOccurenceMatrixSSE_D3(const IplImage *ref, const IplImage *cmp, const int x, const int y);
  void CoOccurenceMatrixSSE_D4(const IplImage *ref, const IplImage *cmp, const int x, const int y);

  void ComputeHaralickFeatures(std::vector<float> &vTexture);
  void ComputeHaralickFeaturesSSE(std::vector<float> &vTexture);

  void ColourHistogram(const IplImage *h, const IplImage *s, const IplImage *v, const int x, const int y, std::vector<float> &vTexture);
  void ColourHistogramSural(const IplImage *h, const IplImage *s, const IplImage *v, const int x, const int y, std::vector<float> &vTexture);

  void LocalExGIntensity(const IplImage *exg, const int x, const int y, std::vector<float> &vTexture);
  void ExGHistogram(const IplImage *exg, const int x, const int y, std::vector<float> &vTexture);

  void CreateGaborFilters(const CvSize size);
  void ReleaseGaborFilters();
  void MultiplyFFT(const CvArr* srcAarr, const CvArr* srcBarr, CvArr* dstarr);
  void SaveDoubleCvMat(const char* filename, CvArr* mat, bool multichannel = false);

  __m128 log_ps(__m128 x);
};

template <class ForwardIterator>
  ForwardIterator absmax_element( ForwardIterator first, ForwardIterator last )
{
  ForwardIterator largest = first;
  if (first==last) return last;
  while (++first!=last)
    if (fabs(*largest)<fabs(*first))    // or: if (comp(*largest,*lowest)) for the comp version
      largest=first;
  return largest;
}

template <class T> const int Texture<T>::m_hsv_shift = 12;
template <class T> const int Texture<T>::m_div_table255[256] = {
    0, 1044480, 522240, 348160, 261120, 208896, 174080, 149211,130560, 116053, 104448, 94953, 87040, 80345, 74606, 69632,
    65280, 61440, 58027, 54973, 52224, 49737, 47476, 45412,43520, 41779, 40172, 38684, 37303, 36017, 34816, 33693,
    32640, 31651, 30720, 29842, 29013, 28229, 27486, 26782,26112, 25475, 24869, 24290, 23738, 23211, 22706, 22223,
    21760, 21316, 20890, 20480, 20086, 19707, 19342, 18991,18651, 18324, 18008, 17703, 17408, 17123, 16846, 16579,
    16320, 16069, 15825, 15589, 15360, 15137, 14921, 14711,14507, 14308, 14115, 13926, 13743, 13565, 13391, 13221,
    13056, 12895, 12738, 12584, 12434, 12288, 12145, 12006,11869, 11736, 11605, 11478, 11353, 11231, 11111, 10995,
    10880, 10768, 10658, 10550, 10445, 10341, 10240, 10141,10043, 9947, 9854, 9761, 9671, 9582, 9495, 9410,
    9326, 9243, 9162, 9082, 9004, 8927, 8852, 8777,8704, 8632, 8561, 8492, 8423, 8356, 8290, 8224,
    8160, 8097, 8034, 7973, 7913, 7853, 7795, 7737,7680, 7624, 7569, 7514, 7461, 7408, 7355, 7304,
    7253, 7203, 7154, 7105, 7057, 7010, 6963, 6917,6872, 6827, 6782, 6739, 6695, 6653, 6611, 6569,
    6528, 6487, 6447, 6408, 6369, 6330, 6292, 6254,6217, 6180, 6144, 6108, 6073, 6037, 6003, 5968,
    5935, 5901, 5868, 5835, 5803, 5771, 5739, 5708,5677, 5646, 5615, 5585, 5556, 5526, 5497, 5468,
    5440, 5412, 5384, 5356, 5329, 5302, 5275, 5249,5222, 5196, 5171, 5145, 5120, 5095, 5070, 5046,
    5022, 4998, 4974, 4950, 4927, 4904, 4881, 4858,4836, 4813, 4791, 4769, 4748, 4726, 4705, 4684,
    4663, 4642, 4622, 4601, 4581, 4561, 4541, 4522,4502, 4483, 4464, 4445, 4426, 4407, 4389, 4370,
    4352, 4334, 4316, 4298, 4281, 4263, 4246, 4229,4212, 4195, 4178, 4161, 4145, 4128, 4112, 4096};

template <class T> Texture<T>::Texture(const CvSize imsize, const int sGraylevels) : 
  m_imsize(imsize),
  m_window(16), 
  m_border(16),
  m_offset(m_window/2),
  m_graylevels(sGraylevels), 
  m_scale((sGraylevels-1)/255.0), 
  m_prob(1.0/(4*(m_window*(m_window-1) + (m_window-1)*(m_window-1)))),
  m_prob0(1.0/(m_window*m_window)),
  m_hueshift(2),
  m_valshift(1),
  m_threshold(40),
  m_gabor_scales(3),//5
  m_gabor_orientations(8),
  m_gabor_fmax(M_PI/2.0),
  m_gabor_sigma(2*M_PI)
{
  m_comatrix = cvCreateImage(cvSize(m_graylevels, m_graylevels), IPL_DEPTH_32F, 1);
  m_comatrixt = cvCreateImage(cvSize(m_graylevels, m_graylevels), IPL_DEPTH_32F, 1);
  m_con = cvCreateImage(cvSize(m_graylevels, m_graylevels), IPL_DEPTH_32F, 1);
  m_idm = cvCreateImage(cvSize(m_graylevels, m_graylevels), IPL_DEPTH_32F, 1);
  m_sum = cvCreateImage(cvSize(m_graylevels, m_graylevels), IPL_DEPTH_32S, 1);
  m_dif = cvCreateImage(cvSize(m_graylevels, m_graylevels), IPL_DEPTH_32S, 1);

  m_h = cvCreateImage(m_imsize, IPL_DEPTH_8U, 1);
  m_s = cvCreateImage(m_imsize, IPL_DEPTH_8U, 1);
  m_v = cvCreateImage(m_imsize, IPL_DEPTH_8U, 1);
  m_tmp = cvCreateImage(m_imsize, IPL_DEPTH_8U, 1);

  m_asum = new float[2*m_graylevels-1];
  m_adif = new float[m_graylevels];
  cvGetRawData(m_comatrix, (uchar**)&m_comatrix_data);
  cvGetRawData(m_sum, (uchar**)&m_sum_data);
  cvGetRawData(m_dif, (uchar**)&m_dif_data);

  for (int i = 0; i < m_graylevels; ++i)
  {
    for (int j = 0; j < m_graylevels; ++j)
    {
      CV_IMAGE_ELEM(m_con, float, i, j) = (i-j)*(i-j);
      CV_IMAGE_ELEM(m_idm, float, i, j) = 1.0/(1.0 + (i-j)*(i-j));
      CV_IMAGE_ELEM(m_sum, int, i, j) = i+j;
      CV_IMAGE_ELEM(m_dif, int, i, j) = abs(i-j);
    }
  }

  for(int i = 0; i < 256; ++i)
    m_div_tableG[i] = cvRound(m_div_table255[i]*(m_graylevels-1)/255.0);

  for(int i = 0; i <= 360; ++i)
    m_map_tableG[i] = cvRound(i*(m_graylevels-1)/360.0);

  m_huehist = new int[(m_graylevels >> m_hueshift)];
  m_valhist = new int[(m_graylevels >> m_valshift)];
  m_colhist = new int[8*4*4];

  mScarf = new Scarf(m_imsize);

  //CreateGaborFilters(m_imsize);
}

template <class T> Texture<T>::~Texture()
{
  cvReleaseImage(&m_h);
  cvReleaseImage(&m_s);
  cvReleaseImage(&m_v);
  cvReleaseImage(&m_tmp);
  cvReleaseImage(&m_comatrix);
  cvReleaseImage(&m_comatrixt);
  cvReleaseImage(&m_idm);
  cvReleaseImage(&m_con);
  cvReleaseImage(&m_sum);
  cvReleaseImage(&m_dif);
  delete [] m_asum;
  delete [] m_adif;
  delete [] m_huehist;
  delete [] m_valhist;
  delete [] m_colhist;
  
  delete mScarf;

  //ReleaseGaborFilters();
}

template <class T> void Texture<T>::DescribeTextureCCM(const IplImage *src, typename std::vector<KeyPoint<T> >::iterator first, typename std::vector<KeyPoint<T> >::iterator last)
{
  for(typename std::vector<KeyPoint<T> >::iterator it = first; it != last; ++it)
  {
    int tlx = it->x - m_offset;
    int tly = it->y - m_offset;
    if(tlx < m_border || tly < m_border || (tlx + m_window) > (m_imsize.width - m_border) || (tly + m_window) > (m_imsize.height - m_border))
      continue;

    switch (it->scale) {
      case 1: CoOccurenceMatrixSSE_D1(src, src, tlx, tly); break;
      case 2: CoOccurenceMatrixSSE_D2(src, src, tlx, tly); break;
      case 3: CoOccurenceMatrixSSE_D3(src, src, tlx, tly); break;
      case 4: CoOccurenceMatrixSSE_D4(src, src, tlx, tly); break;
      default: CoOccurenceMatrix(src, src, tlx, tly, it->scale); break;
    }
    ComputeHaralickFeaturesSSE(it->vTexture);
  }
}

template <class T> void Texture<T>::DescribeTextureSCARF(const IplImage *src, typename std::vector<KeyPoint<T> >::iterator first, typename std::vector<KeyPoint<T> >::iterator last)
{
  uint8_t descriptor[64];
  mScarf->LoadImage(src);

  for(typename std::vector<KeyPoint<T> >::iterator it = first; it != last; ++it)
  {
    if(it->x < m_border || it->y < m_border || it->x > (m_imsize.width - m_border) || it->y > (m_imsize.height - m_border))
      continue;

    /*float max_response = 0.0;
    unsigned char scale = 0;
    mScarf->FilterResponses(it->x, it->y, 3, max_response, scale);
    int val = std::min(std::max((int)(max_response + 0.5) + 128, 0), 255);
    it->vTexture.push_back(val);*/
    
    mScarf->DescribePoint(it->x, it->y, 2, descriptor);//it->scale
    for(int i = 0; i < 60; ++i)
      it->vTexture.push_back((float)descriptor[i]);
  }
}

template <class T> void Texture<T>::DescribeTextureGabor(const IplImage *src, typename std::vector<KeyPoint<T> >::iterator first, typename std::vector<KeyPoint<T> >::iterator last)
{
  CvSize filter_size = cvGetSize(m_gabor[0]);
	CvMat* gimg = cvCreateMat(m_imsize.height, m_imsize.width, CV_64F);
	CvMat* gout = cvCreateMat(filter_size.height, filter_size.width, CV_64FC2);
	CvMat* imdft = cvCreateMat(filter_size.height, filter_size.width, CV_64FC2);

	cvZero(imdft);
	for(int i = 0; i < m_imsize.height; i++)
  {
		for(int j = 0; j < m_imsize.width; j++)
		{
			((double*)(imdft->data.ptr + imdft->step*i))[j*2] = (double)(CV_IMAGE_ELEM(src, uchar, i, j));
			((double*)(imdft->data.ptr + imdft->step*i))[j*2+1] = 0.0;
		}
  }
	cvDFT(imdft, imdft, CV_DXT_FORWARD);

 	for (int i = 0; i < m_gabor_scales; i++)
	{
		for (int j = 0; j < m_gabor_orientations; j++) 
		{
	    MultiplyFFT(imdft, m_gabor[i*m_gabor_orientations+j], gout);
 			cvDFT(gout, gout, CV_DXT_INVERSE);

      for(int y = 0; y < m_imsize.height;  y++) 
      {
        for(int x = 0; x < m_imsize.width; x++)
        {
          ((double*)(gimg->data.ptr + gimg->step*y))[x] =
		        sqrt( ((double*)(gout->data.ptr + gout->step*(y+m_imsize.height/2)))[(x+m_imsize.width/2)*2]*
		         ((double*)(gout->data.ptr + gout->step*(y+m_imsize.height/2)))[(x+m_imsize.width/2)*2]+
		         ((double*)(gout->data.ptr + gout->step*(y+m_imsize.height/2)))[(x+m_imsize.width/2)*2+1]*
		         ((double*)(gout->data.ptr + gout->step*(y+m_imsize.height/2)))[(x+m_imsize.width/2)*2+1]);
        }
      }

			char coutfilename[512];
			sprintf(coutfilename,"gwt%d_%d.bmp",i,j);
			SaveDoubleCvMat(coutfilename, gimg);

      CvScalar mean = cvAvg(gimg);
      cvSubS(gimg, mean, gimg);
      cvNormalize(gimg, gimg);

      for(typename std::vector<KeyPoint<T> >::iterator it = first; it != last; ++it)
      {
        if(it->x < m_border || it->y < m_border || it->x > (m_imsize.width-m_border) || it->y > (m_imsize.height-m_border))
          continue;

        it->vTexture.push_back(cvGetReal2D(gimg, it->y, it->x));

        if(j+1 == m_gabor_orientations) // Last orientation for this scale - circularly shift to get dominant orientation first
        {
          std::vector<float>::iterator first = it->vTexture.end()-m_gabor_orientations;
          std::vector<float>::iterator last = it->vTexture.end();
          /*std::cout << "Before: " << std::endl;
          for(std::vector<float>::iterator cit = first; cit != last; ++cit)
            std::cout << *cit << " ";
          std::cout << std::endl;*/
          rotate(first, absmax_element(first, last), last);
          /*std::cout << "After: " << std::endl;
          for(std::vector<float>::iterator cit = first; cit != last; ++cit)
            std::cout << *cit << " ";
          std::cout << std::endl;*/
        }
      }
		}
	}

	cvReleaseMat(&imdft);
  cvReleaseMat(&gout);		 	 
  cvReleaseMat(&gimg);
}



template <class T> void Texture<T>::DescribeColour(const IplImage *hue, const IplImage *saturation, const IplImage *value, typename std::vector<KeyPoint<T> >::iterator first, typename std::vector<KeyPoint<T> >::iterator last)
{
  for(typename std::vector<KeyPoint<T> >::iterator it = first; it != last; ++it)
  {
    int tlx = it->x - m_offset;
    int tly = it->y - m_offset;
    if(tlx < m_border || tly < m_border || (tlx + m_window) > (m_imsize.width - m_border) || (tly + m_window) > (m_imsize.height - m_border))
      continue;

    ColourHistogram(hue, saturation, value, tlx, tly, it->vTexture);
    //ColourHistogramSural(hue, saturation, value, tlx, tly, it->vTexture);
  }
}

template <class T> void Texture<T>::DescribeColour(const IplImage *rgb, typename std::vector<KeyPoint<T> >::iterator first, typename std::vector<KeyPoint<T> >::iterator last)
{
  for(typename std::vector<KeyPoint<T> >::iterator it = first; it != last; ++it)
  {
    int tlx = it->x - m_offset;
    int tly = it->y - m_offset;
    if(tlx < m_border || tly < m_border || (tlx + m_window) > (m_imsize.width - m_border) || (tly + m_window) > (m_imsize.height - m_border))
      continue;

    memset(m_colhist, 0, (4*4*4)*sizeof(int)); //TODO: not using whole array!

    for (int i = tly; i < (tly+m_window); ++i)
    {
      for (int j = tlx; j < (tlx+m_window); ++j)
      {
        m_colhist[(4*4)*(CV_IMAGE_ELEM(rgb, uchar, i, 3*j+2) >> 6) + 4*(CV_IMAGE_ELEM(rgb, uchar, i, 3*j+1) >> 6) + (CV_IMAGE_ELEM(rgb, uchar, i, 3*j) >> 6)]++;        
      }
    }

    it->vTexture.insert(it->vTexture.end(), m_colhist, m_colhist + (4*4*4));
  }
}

template <class T> void Texture<T>::DescribeVegetation(const IplImage *src, typename std::vector<KeyPoint<T> >::iterator first, typename std::vector<KeyPoint<T> >::iterator last)
{
  for(typename std::vector<KeyPoint<T> >::iterator it = first; it != last; ++it)
  {
    int tlx = it->x - m_offset;
    int tly = it->y - m_offset;
    if(tlx < m_border || tly < m_border || (tlx + m_window) > (m_imsize.width - m_border) || (tly + m_window) > (m_imsize.height - m_border))
      continue;

    //LocalExGIntensity(src, tlx, tly, it->vTexture);
    ExGHistogram(src, tlx, tly, it->vTexture);
  }
}

template <class T> void Texture<T>::ColourHistogramSural(const IplImage *h, const IplImage *s, const IplImage *v, const int x, const int y, std::vector<float> &vTexture)
{
  //Based on Sural et. al. (2002) "SEGMENTATION AND HISTOGRAM GENERATION USING THE HSV COLOR SPACE FOR IMAGE RETRIEVAL"

  memset(m_huehist, 0, (m_graylevels >> m_hueshift)*sizeof(int));
  memset(m_valhist, 0, (m_graylevels >> m_valshift)*sizeof(int));

  for (int i = y; i < (y+m_window); ++i)
  {
    for (int j = x; j < (x+m_window); ++j)
    {
      //std::cout << (int)CV_IMAGE_ELEM(s, uchar, i, j) << " " << (int)CV_IMAGE_ELEM(v, uchar, i, j) << std::endl;

      if(CV_IMAGE_ELEM(s, uchar, i, j) > (m_graylevels - (0.8 * CV_IMAGE_ELEM(v, uchar, i, j))))
        m_huehist[CV_IMAGE_ELEM(h, uchar, i, j) >> m_hueshift]++;        
      else
        m_valhist[CV_IMAGE_ELEM(v, uchar, i, j) >> m_valshift]++;        
    }
  }

  vTexture.insert(vTexture.end(), m_huehist, m_huehist + (m_graylevels >> m_hueshift));
  vTexture.insert(vTexture.end(), m_valhist, m_valhist + (m_graylevels >> m_valshift));
}

template <class T> void Texture<T>::ColourHistogram(const IplImage *h, const IplImage *s, const IplImage *v, const int x, const int y, std::vector<float> &vTexture)
{
  memset(m_colhist, 0, (8*4*4)*sizeof(int));

  for (int i = y; i < (y+m_window); ++i)
  {
    for (int j = x; j < (x+m_window); ++j)
    {
      m_colhist[(8*4)*(CV_IMAGE_ELEM(v, uchar, i, j) >> 4) + 4*(CV_IMAGE_ELEM(s, uchar, i, j) >> 4) + (CV_IMAGE_ELEM(h, uchar, i, j) >> 3)]++;        
    }
  }

  vTexture.insert(vTexture.end(), m_colhist, m_colhist + (8*4*4));
}

template <class T> void Texture<T>::LocalExGIntensity(const IplImage *exg, const int x, const int y, std::vector<float> &vTexture)
{
  //Based on idea from Naeem et. al. (2007) "Weed Classification using Two Dimensional Weed Coverage Rate (2D-WCR) for Real-Time Selective Herbicide Applications"
  int sum = 0;
  for (int i = y; i < (y+m_window); ++i)
    for (int j = x; j < (x+m_window); ++j)
      sum += CV_IMAGE_ELEM(exg, uchar, i, j);

  vTexture.push_back(sum);
}

template <class T> void Texture<T>::ExGHistogram(const IplImage *exg, const int x, const int y, std::vector<float> &vTexture)
{
  memset(m_colhist, 0, 16*sizeof(int));

  for (int i = y; i < (y+m_window); ++i)
    for (int j = x; j < (x+m_window); ++j)
      m_colhist[CV_IMAGE_ELEM(exg, uchar, i, j) >> 4]++;        

  vTexture.insert(vTexture.end(), m_colhist, m_colhist + 16);
}

template <class T> void Texture<T>::CoOccurenceMatrix(const IplImage *ref, const IplImage *cmp, const int x, const int y, const int d)
{
  cvSetZero(m_comatrix);

  for (int i = y; i < (y+m_window); ++i)
  {
    for (int j = x; j < (x+m_window); ++j)
    {
      if(d == 0)
      {
        CV_IMAGE_ELEM(m_comatrix, float, (int)CV_IMAGE_ELEM(ref, uchar, i, j), (int)CV_IMAGE_ELEM(cmp, uchar, i, j)) += m_prob0;
      }
      else
      {
        if(j+d < (x+m_window))
        {
          CV_IMAGE_ELEM(m_comatrix, float, (int)CV_IMAGE_ELEM(ref, uchar, i, j), (int)CV_IMAGE_ELEM(cmp, uchar, i, j+d)) += m_prob;

          if(i-d >= y)
            CV_IMAGE_ELEM(m_comatrix, float, (int)CV_IMAGE_ELEM(ref, uchar, i, j), (int)CV_IMAGE_ELEM(cmp, uchar, i-d, j+d)) += m_prob;
        }

        if(i+d < (y+m_window))
        {
          CV_IMAGE_ELEM(m_comatrix, float, (int)CV_IMAGE_ELEM(ref, uchar, i, j), (int)CV_IMAGE_ELEM(cmp, uchar, i+d, j)) += m_prob;

          if(j+d < (x+m_window))
            CV_IMAGE_ELEM(m_comatrix, float, (int)CV_IMAGE_ELEM(ref, uchar, i, j), (int)CV_IMAGE_ELEM(cmp, uchar, i+d, j+d)) += m_prob;
        }
      }
    }
  }

  cvTranspose(m_comatrix, m_comatrixt);
  cvAdd(m_comatrix, m_comatrixt, m_comatrix);
}


//constraints: src.width%16 = 0, greylevels <= 256 (and src image depth = 8bits)
template <class T> void Texture<T>::CoOccurenceMatrixSSE_D0(const IplImage *ref, const IplImage *cmp, const int x, const int y)
{
  assert(ref != cmp); //Otherwise the result is nonsense
  cvSetZero(m_comatrix);
  __m128i zeros = _mm_setzero_si128();
  __m128i levels = _mm_set1_epi16(m_graylevels);

  for (int i = 0; i < m_window; ++i)
  { 
    uchar *p_ref = &CV_IMAGE_ELEM(ref, uchar, y+i, x);
    uchar *p_cmp = &CV_IMAGE_ELEM(cmp, uchar, y+i, x);

    for (int j = 0; j < m_window; j += 16)
    {
      __m128i rpp = (__m128i)_mm_loadu_ps((const float *)p_ref);
      __m128i rpplo = _mm_unpacklo_epi8(rpp, zeros);
      __m128i rpphi = _mm_unpackhi_epi8(rpp, zeros);

      __m128i cpp = (__m128i)_mm_loadu_ps((const float *)p_cmp);
      __m128i cpplo = _mm_add_epi16(_mm_mullo_epi16(levels, rpplo), _mm_unpacklo_epi8(cpp, zeros));
      __m128i cpphi = _mm_add_epi16(_mm_mullo_epi16(levels, rpphi), _mm_unpackhi_epi8(cpp, zeros));

      m_comatrix_data[_mm_extract_epi16(cpplo, 0)] += m_prob0;
      m_comatrix_data[_mm_extract_epi16(cpphi, 0)] += m_prob0;
      m_comatrix_data[_mm_extract_epi16(cpplo, 1)] += m_prob0;
      m_comatrix_data[_mm_extract_epi16(cpphi, 1)] += m_prob0;
      m_comatrix_data[_mm_extract_epi16(cpplo, 2)] += m_prob0;
      m_comatrix_data[_mm_extract_epi16(cpphi, 2)] += m_prob0;
      m_comatrix_data[_mm_extract_epi16(cpplo, 3)] += m_prob0;
      m_comatrix_data[_mm_extract_epi16(cpphi, 3)] += m_prob0;
      m_comatrix_data[_mm_extract_epi16(cpplo, 4)] += m_prob0;
      m_comatrix_data[_mm_extract_epi16(cpphi, 4)] += m_prob0;
      m_comatrix_data[_mm_extract_epi16(cpplo, 5)] += m_prob0;
      m_comatrix_data[_mm_extract_epi16(cpphi, 5)] += m_prob0;
      m_comatrix_data[_mm_extract_epi16(cpplo, 6)] += m_prob0;
      m_comatrix_data[_mm_extract_epi16(cpphi, 6)] += m_prob0;
      m_comatrix_data[_mm_extract_epi16(cpplo, 7)] += m_prob0;
      m_comatrix_data[_mm_extract_epi16(cpphi, 7)] += m_prob0;

      p_ref += 16;
      p_cmp += 16;
    }
  }

  cvTranspose(m_comatrix, m_comatrixt);
  cvAdd(m_comatrix, m_comatrixt, m_comatrix);
}


template <class T> void Texture<T>::CoOccurenceMatrixSSE_D1(const IplImage *ref, const IplImage *cmp, const int x, const int y)
{
  const int d = 1;
  cvSetZero(m_comatrix);
  __m128i zeros = _mm_setzero_si128();
  __m128i levels = _mm_set1_epi16(m_graylevels);

  for (int i = 0; i < m_window; ++i)
  { 
    uchar *p_ref = &CV_IMAGE_ELEM(ref, uchar, y+i, x);
    uchar *p_cmp = &CV_IMAGE_ELEM(cmp, uchar, y+i, x);

    for (int j = 0; j < m_window; j += 16)
    {
      __m128i rpp = (__m128i)_mm_loadu_ps((const float *)p_ref);
      __m128i rpplo = _mm_unpacklo_epi8(rpp, zeros);
      __m128i rpphi = _mm_unpackhi_epi8(rpp, zeros);

      __m128i hrs = (__m128i)_mm_loadu_ps((const float *)(p_cmp + d)); //Will go out of bounds (by d)
      __m128i hrslo = _mm_add_epi16(_mm_mullo_epi16(levels, rpplo), _mm_unpacklo_epi8(hrs, zeros));
      __m128i hrshi = _mm_add_epi16(_mm_mullo_epi16(levels, rpphi), _mm_unpackhi_epi8(hrs, zeros));

      if(j == (m_window-16))
      {
        m_comatrix_data[_mm_extract_epi16(hrslo, 0)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrslo, 1)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrslo, 2)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrslo, 3)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrslo, 4)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrslo, 5)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrslo, 6)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrslo, 7)] += m_prob;
        //if(k < (8-d))
        m_comatrix_data[_mm_extract_epi16(hrshi, 0)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrshi, 1)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrshi, 2)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrshi, 3)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrshi, 4)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrshi, 5)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrshi, 6)] += m_prob;
      }
      else
      {
        m_comatrix_data[_mm_extract_epi16(hrslo, 0)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrshi, 0)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrslo, 1)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrshi, 1)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrslo, 2)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrshi, 2)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrslo, 3)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrshi, 3)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrslo, 4)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrshi, 4)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrslo, 5)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrshi, 5)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrslo, 6)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrshi, 6)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrslo, 7)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrshi, 7)] += m_prob;
      }

      if(i < (m_window-d))
      {
        __m128i vrs = (__m128i)_mm_loadu_ps((const float *)(p_cmp + d*(cmp->width)));
        __m128i vrslo = _mm_add_epi16(_mm_mullo_epi16(levels, rpplo), _mm_unpacklo_epi8(vrs, zeros));
        __m128i vrshi = _mm_add_epi16(_mm_mullo_epi16(levels, rpphi), _mm_unpackhi_epi8(vrs, zeros));

        __m128i dds = (__m128i)_mm_loadu_ps((const float *)(p_cmp + d*(cmp->width) + d)); //Will go out of bounds (by d)
        __m128i ddslo = _mm_add_epi16(_mm_mullo_epi16(levels, rpplo), _mm_unpacklo_epi8(dds, zeros));
        __m128i ddshi = _mm_add_epi16(_mm_mullo_epi16(levels, rpphi), _mm_unpackhi_epi8(dds, zeros));

        m_comatrix_data[_mm_extract_epi16(vrslo, 0)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(vrshi, 0)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(ddslo, 0)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(vrslo, 1)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(vrshi, 1)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(ddslo, 1)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(vrslo, 2)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(vrshi, 2)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(ddslo, 2)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(vrslo, 3)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(vrshi, 3)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(ddslo, 3)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(vrslo, 4)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(vrshi, 4)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(ddslo, 4)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(vrslo, 5)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(vrshi, 5)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(ddslo, 5)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(vrslo, 6)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(vrshi, 6)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(ddslo, 6)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(vrslo, 7)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(vrshi, 7)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(ddslo, 7)] += m_prob;

        if(j == (m_window-16))
        {
          //if(k < (8-d))
          m_comatrix_data[_mm_extract_epi16(ddshi, 0)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(ddshi, 1)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(ddshi, 2)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(ddshi, 3)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(ddshi, 4)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(ddshi, 5)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(ddshi, 6)] += m_prob;
        }
        else
        {
          m_comatrix_data[_mm_extract_epi16(ddshi, 0)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(ddshi, 1)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(ddshi, 2)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(ddshi, 3)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(ddshi, 4)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(ddshi, 5)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(ddshi, 6)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(ddshi, 7)] += m_prob;
        }
      }

      if(i >= d)
      {
        __m128i dus = (__m128i)_mm_loadu_ps((const float *)(p_cmp - d*(cmp->width) + d));
        __m128i duslo = _mm_add_epi16(_mm_mullo_epi16(levels, rpplo), _mm_unpacklo_epi8(dus, zeros));
        __m128i dushi = _mm_add_epi16(_mm_mullo_epi16(levels, rpphi), _mm_unpackhi_epi8(dus, zeros));

        if(j == (m_window-16))
        {
          m_comatrix_data[_mm_extract_epi16(duslo, 0)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(duslo, 1)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(duslo, 2)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(duslo, 3)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(duslo, 4)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(duslo, 5)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(duslo, 6)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(duslo, 7)] += m_prob;
          //if(k < (8-d))
          m_comatrix_data[_mm_extract_epi16(dushi, 0)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(dushi, 1)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(dushi, 2)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(dushi, 3)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(dushi, 4)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(dushi, 5)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(dushi, 6)] += m_prob;
        }
        else
        {
          m_comatrix_data[_mm_extract_epi16(duslo, 0)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(dushi, 0)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(duslo, 1)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(dushi, 1)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(duslo, 2)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(dushi, 2)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(duslo, 3)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(dushi, 3)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(duslo, 4)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(dushi, 4)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(duslo, 5)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(dushi, 5)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(duslo, 6)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(dushi, 6)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(duslo, 7)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(dushi, 7)] += m_prob;
        }
      }

      p_ref += 16;
      p_cmp += 16;
    }
  }

  cvTranspose(m_comatrix, m_comatrixt);
  cvAdd(m_comatrix, m_comatrixt, m_comatrix);
}

template <class T> void Texture<T>::CoOccurenceMatrixSSE_D2(const IplImage *ref, const IplImage *cmp, const int x, const int y)
{
  const int d = 2;
  cvSetZero(m_comatrix);
  __m128i zeros = _mm_setzero_si128();
  __m128i levels = _mm_set1_epi16(m_graylevels);

  for (int i = 0; i < m_window; ++i)
  { 
    uchar *p_ref = &CV_IMAGE_ELEM(ref, uchar, y+i, x);
    uchar *p_cmp = &CV_IMAGE_ELEM(cmp, uchar, y+i, x);

    for (int j = 0; j < m_window; j += 16)
    {
      __m128i rpp = (__m128i)_mm_loadu_ps((const float *)p_ref);
      __m128i rpplo = _mm_unpacklo_epi8(rpp, zeros);
      __m128i rpphi = _mm_unpackhi_epi8(rpp, zeros);

      __m128i hrs = (__m128i)_mm_loadu_ps((const float *)(p_cmp + d)); //Will go out of bounds (by d)
      __m128i hrslo = _mm_add_epi16(_mm_mullo_epi16(levels, rpplo), _mm_unpacklo_epi8(hrs, zeros));
      __m128i hrshi = _mm_add_epi16(_mm_mullo_epi16(levels, rpphi), _mm_unpackhi_epi8(hrs, zeros));

      if(j == (m_window-16))
      {
        m_comatrix_data[_mm_extract_epi16(hrslo, 0)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrslo, 1)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrslo, 2)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrslo, 3)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrslo, 4)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrslo, 5)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrslo, 6)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrslo, 7)] += m_prob;
        //if(k < (8-d))
        m_comatrix_data[_mm_extract_epi16(hrshi, 0)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrshi, 1)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrshi, 2)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrshi, 3)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrshi, 4)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrshi, 5)] += m_prob;
      }
      else
      {
        m_comatrix_data[_mm_extract_epi16(hrslo, 0)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrshi, 0)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrslo, 1)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrshi, 1)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrslo, 2)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrshi, 2)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrslo, 3)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrshi, 3)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrslo, 4)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrshi, 4)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrslo, 5)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrshi, 5)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrslo, 6)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrshi, 6)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrslo, 7)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrshi, 7)] += m_prob;
      }

      if(i < (m_window-d))
      {
        __m128i vrs = (__m128i)_mm_loadu_ps((const float *)(p_cmp + d*(cmp->width)));
        __m128i vrslo = _mm_add_epi16(_mm_mullo_epi16(levels, rpplo), _mm_unpacklo_epi8(vrs, zeros));
        __m128i vrshi = _mm_add_epi16(_mm_mullo_epi16(levels, rpphi), _mm_unpackhi_epi8(vrs, zeros));

        __m128i dds = (__m128i)_mm_loadu_ps((const float *)(p_cmp + d*(cmp->width) + d)); //Will go out of bounds (by d)
        __m128i ddslo = _mm_add_epi16(_mm_mullo_epi16(levels, rpplo), _mm_unpacklo_epi8(dds, zeros));
        __m128i ddshi = _mm_add_epi16(_mm_mullo_epi16(levels, rpphi), _mm_unpackhi_epi8(dds, zeros));

        m_comatrix_data[_mm_extract_epi16(vrslo, 0)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(vrshi, 0)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(ddslo, 0)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(vrslo, 1)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(vrshi, 1)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(ddslo, 1)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(vrslo, 2)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(vrshi, 2)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(ddslo, 2)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(vrslo, 3)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(vrshi, 3)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(ddslo, 3)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(vrslo, 4)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(vrshi, 4)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(ddslo, 4)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(vrslo, 5)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(vrshi, 5)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(ddslo, 5)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(vrslo, 6)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(vrshi, 6)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(ddslo, 6)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(vrslo, 7)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(vrshi, 7)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(ddslo, 7)] += m_prob;

        if(j == (m_window-16))
        {
          //if(k < (8-d))
          m_comatrix_data[_mm_extract_epi16(ddshi, 0)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(ddshi, 1)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(ddshi, 2)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(ddshi, 3)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(ddshi, 4)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(ddshi, 5)] += m_prob;
        }
        else
        {
          m_comatrix_data[_mm_extract_epi16(ddshi, 0)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(ddshi, 1)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(ddshi, 2)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(ddshi, 3)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(ddshi, 4)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(ddshi, 5)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(ddshi, 6)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(ddshi, 7)] += m_prob;
        }
      }

      if(i >= d)
      {
        __m128i dus = (__m128i)_mm_loadu_ps((const float *)(p_cmp - d*(cmp->width) + d));
        __m128i duslo = _mm_add_epi16(_mm_mullo_epi16(levels, rpplo), _mm_unpacklo_epi8(dus, zeros));
        __m128i dushi = _mm_add_epi16(_mm_mullo_epi16(levels, rpphi), _mm_unpackhi_epi8(dus, zeros));

        if(j == (m_window-16))
        {
          m_comatrix_data[_mm_extract_epi16(duslo, 0)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(duslo, 1)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(duslo, 2)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(duslo, 3)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(duslo, 4)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(duslo, 5)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(duslo, 6)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(duslo, 7)] += m_prob;
          //if(k < (8-d))
          m_comatrix_data[_mm_extract_epi16(dushi, 0)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(dushi, 1)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(dushi, 2)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(dushi, 3)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(dushi, 4)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(dushi, 5)] += m_prob;
        }
        else
        {
          m_comatrix_data[_mm_extract_epi16(duslo, 0)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(dushi, 0)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(duslo, 1)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(dushi, 1)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(duslo, 2)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(dushi, 2)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(duslo, 3)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(dushi, 3)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(duslo, 4)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(dushi, 4)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(duslo, 5)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(dushi, 5)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(duslo, 6)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(dushi, 6)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(duslo, 7)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(dushi, 7)] += m_prob;
        }
      }

      p_ref += 16;
      p_cmp += 16;
    }
  }

  cvTranspose(m_comatrix, m_comatrixt);
  cvAdd(m_comatrix, m_comatrixt, m_comatrix);
}

template <class T> void Texture<T>::CoOccurenceMatrixSSE_D3(const IplImage *ref, const IplImage *cmp, const int x, const int y)
{
  const int d = 3;
  cvSetZero(m_comatrix);
  __m128i zeros = _mm_setzero_si128();
  __m128i levels = _mm_set1_epi16(m_graylevels);

  for (int i = 0; i < m_window; ++i)
  { 
    uchar *p_ref = &CV_IMAGE_ELEM(ref, uchar, y+i, x);
    uchar *p_cmp = &CV_IMAGE_ELEM(cmp, uchar, y+i, x);

    for (int j = 0; j < m_window; j += 16)
    {
      __m128i rpp = (__m128i)_mm_loadu_ps((const float *)p_ref);
      __m128i rpplo = _mm_unpacklo_epi8(rpp, zeros);
      __m128i rpphi = _mm_unpackhi_epi8(rpp, zeros);

      __m128i hrs = (__m128i)_mm_loadu_ps((const float *)(p_cmp + d)); //Will go out of bounds (by d)
      __m128i hrslo = _mm_add_epi16(_mm_mullo_epi16(levels, rpplo), _mm_unpacklo_epi8(hrs, zeros));
      __m128i hrshi = _mm_add_epi16(_mm_mullo_epi16(levels, rpphi), _mm_unpackhi_epi8(hrs, zeros));

      if(j == (m_window-16))
      {
        m_comatrix_data[_mm_extract_epi16(hrslo, 0)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrslo, 1)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrslo, 2)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrslo, 3)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrslo, 4)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrslo, 5)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrslo, 6)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrslo, 7)] += m_prob;
        //if(k < (8-d))
        m_comatrix_data[_mm_extract_epi16(hrshi, 0)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrshi, 1)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrshi, 2)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrshi, 3)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrshi, 4)] += m_prob;
      }
      else
      {
        m_comatrix_data[_mm_extract_epi16(hrslo, 0)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrshi, 0)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrslo, 1)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrshi, 1)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrslo, 2)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrshi, 2)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrslo, 3)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrshi, 3)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrslo, 4)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrshi, 4)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrslo, 5)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrshi, 5)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrslo, 6)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrshi, 6)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrslo, 7)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrshi, 7)] += m_prob;
      }

      if(i < (m_window-d))
      {
        __m128i vrs = (__m128i)_mm_loadu_ps((const float *)(p_cmp + d*(cmp->width)));
        __m128i vrslo = _mm_add_epi16(_mm_mullo_epi16(levels, rpplo), _mm_unpacklo_epi8(vrs, zeros));
        __m128i vrshi = _mm_add_epi16(_mm_mullo_epi16(levels, rpphi), _mm_unpackhi_epi8(vrs, zeros));

        __m128i dds = (__m128i)_mm_loadu_ps((const float *)(p_cmp + d*(cmp->width) + d)); //Will go out of bounds (by d)
        __m128i ddslo = _mm_add_epi16(_mm_mullo_epi16(levels, rpplo), _mm_unpacklo_epi8(dds, zeros));
        __m128i ddshi = _mm_add_epi16(_mm_mullo_epi16(levels, rpphi), _mm_unpackhi_epi8(dds, zeros));

        m_comatrix_data[_mm_extract_epi16(vrslo, 0)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(vrshi, 0)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(ddslo, 0)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(vrslo, 1)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(vrshi, 1)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(ddslo, 1)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(vrslo, 2)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(vrshi, 2)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(ddslo, 2)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(vrslo, 3)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(vrshi, 3)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(ddslo, 3)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(vrslo, 4)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(vrshi, 4)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(ddslo, 4)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(vrslo, 5)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(vrshi, 5)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(ddslo, 5)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(vrslo, 6)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(vrshi, 6)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(ddslo, 6)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(vrslo, 7)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(vrshi, 7)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(ddslo, 7)] += m_prob;

        if(j == (m_window-16))
        {
          //if(k < (8-d))
          m_comatrix_data[_mm_extract_epi16(ddshi, 0)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(ddshi, 1)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(ddshi, 2)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(ddshi, 3)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(ddshi, 4)] += m_prob;
        }
        else
        {
          m_comatrix_data[_mm_extract_epi16(ddshi, 0)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(ddshi, 1)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(ddshi, 2)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(ddshi, 3)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(ddshi, 4)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(ddshi, 5)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(ddshi, 6)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(ddshi, 7)] += m_prob;
        }
      }

      if(i >= d)
      {
        __m128i dus = (__m128i)_mm_loadu_ps((const float *)(p_cmp - d*(cmp->width) + d));
        __m128i duslo = _mm_add_epi16(_mm_mullo_epi16(levels, rpplo), _mm_unpacklo_epi8(dus, zeros));
        __m128i dushi = _mm_add_epi16(_mm_mullo_epi16(levels, rpphi), _mm_unpackhi_epi8(dus, zeros));

        if(j == (m_window-16))
        {
          m_comatrix_data[_mm_extract_epi16(duslo, 0)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(duslo, 1)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(duslo, 2)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(duslo, 3)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(duslo, 4)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(duslo, 5)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(duslo, 6)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(duslo, 7)] += m_prob;
          //if(k < (8-d))
          m_comatrix_data[_mm_extract_epi16(dushi, 0)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(dushi, 1)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(dushi, 2)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(dushi, 3)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(dushi, 4)] += m_prob;
        }
        else
        {
          m_comatrix_data[_mm_extract_epi16(duslo, 0)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(dushi, 0)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(duslo, 1)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(dushi, 1)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(duslo, 2)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(dushi, 2)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(duslo, 3)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(dushi, 3)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(duslo, 4)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(dushi, 4)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(duslo, 5)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(dushi, 5)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(duslo, 6)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(dushi, 6)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(duslo, 7)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(dushi, 7)] += m_prob;
        }
      }

      p_ref += 16;
      p_cmp += 16;
    }
  }

  cvTranspose(m_comatrix, m_comatrixt);
  cvAdd(m_comatrix, m_comatrixt, m_comatrix);
}

template <class T> void Texture<T>::CoOccurenceMatrixSSE_D4(const IplImage *ref, const IplImage *cmp, const int x, const int y)
{
  const int d = 4;
  cvSetZero(m_comatrix);
  __m128i zeros = _mm_setzero_si128();
  __m128i levels = _mm_set1_epi16(m_graylevels);

  for (int i = 0; i < m_window; ++i)
  { 
    uchar *p_ref = &CV_IMAGE_ELEM(ref, uchar, y+i, x);
    uchar *p_cmp = &CV_IMAGE_ELEM(cmp, uchar, y+i, x);

    for (int j = 0; j < m_window; j += 16)
    {
      __m128i rpp = (__m128i)_mm_loadu_ps((const float *)p_ref);
      __m128i rpplo = _mm_unpacklo_epi8(rpp, zeros);
      __m128i rpphi = _mm_unpackhi_epi8(rpp, zeros);

      __m128i hrs = (__m128i)_mm_loadu_ps((const float *)(p_cmp + d)); //Will go out of bounds (by d)
      __m128i hrslo = _mm_add_epi16(_mm_mullo_epi16(levels, rpplo), _mm_unpacklo_epi8(hrs, zeros));
      __m128i hrshi = _mm_add_epi16(_mm_mullo_epi16(levels, rpphi), _mm_unpackhi_epi8(hrs, zeros));

      if(j == (m_window-16))
      {
        m_comatrix_data[_mm_extract_epi16(hrslo, 0)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrslo, 1)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrslo, 2)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrslo, 3)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrslo, 4)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrslo, 5)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrslo, 6)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrslo, 7)] += m_prob;
        //if(k < (8-d))
        m_comatrix_data[_mm_extract_epi16(hrshi, 0)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrshi, 1)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrshi, 2)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrshi, 3)] += m_prob;
      }
      else
      {
        m_comatrix_data[_mm_extract_epi16(hrslo, 0)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrshi, 0)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrslo, 1)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrshi, 1)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrslo, 2)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrshi, 2)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrslo, 3)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrshi, 3)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrslo, 4)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrshi, 4)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrslo, 5)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrshi, 5)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrslo, 6)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrshi, 6)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrslo, 7)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(hrshi, 7)] += m_prob;
      }

      if(i < (m_window-d))
      {
        __m128i vrs = (__m128i)_mm_loadu_ps((const float *)(p_cmp + d*(cmp->width)));
        __m128i vrslo = _mm_add_epi16(_mm_mullo_epi16(levels, rpplo), _mm_unpacklo_epi8(vrs, zeros));
        __m128i vrshi = _mm_add_epi16(_mm_mullo_epi16(levels, rpphi), _mm_unpackhi_epi8(vrs, zeros));

        __m128i dds = (__m128i)_mm_loadu_ps((const float *)(p_cmp + d*(cmp->width) + d)); //Will go out of bounds (by d)
        __m128i ddslo = _mm_add_epi16(_mm_mullo_epi16(levels, rpplo), _mm_unpacklo_epi8(dds, zeros));
        __m128i ddshi = _mm_add_epi16(_mm_mullo_epi16(levels, rpphi), _mm_unpackhi_epi8(dds, zeros));

        m_comatrix_data[_mm_extract_epi16(vrslo, 0)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(vrshi, 0)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(ddslo, 0)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(vrslo, 1)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(vrshi, 1)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(ddslo, 1)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(vrslo, 2)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(vrshi, 2)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(ddslo, 2)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(vrslo, 3)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(vrshi, 3)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(ddslo, 3)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(vrslo, 4)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(vrshi, 4)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(ddslo, 4)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(vrslo, 5)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(vrshi, 5)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(ddslo, 5)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(vrslo, 6)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(vrshi, 6)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(ddslo, 6)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(vrslo, 7)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(vrshi, 7)] += m_prob;
        m_comatrix_data[_mm_extract_epi16(ddslo, 7)] += m_prob;

        if(j == (m_window-16))
        {
          //if(k < (8-d))
          m_comatrix_data[_mm_extract_epi16(ddshi, 0)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(ddshi, 1)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(ddshi, 2)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(ddshi, 3)] += m_prob;
        }
        else
        {
          m_comatrix_data[_mm_extract_epi16(ddshi, 0)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(ddshi, 1)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(ddshi, 2)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(ddshi, 3)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(ddshi, 4)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(ddshi, 5)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(ddshi, 6)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(ddshi, 7)] += m_prob;
        }
      }

      if(i >= d)
      {
        __m128i dus = (__m128i)_mm_loadu_ps((const float *)(p_cmp - d*(cmp->width) + d));
        __m128i duslo = _mm_add_epi16(_mm_mullo_epi16(levels, rpplo), _mm_unpacklo_epi8(dus, zeros));
        __m128i dushi = _mm_add_epi16(_mm_mullo_epi16(levels, rpphi), _mm_unpackhi_epi8(dus, zeros));

        if(j == (m_window-16))
        {
          m_comatrix_data[_mm_extract_epi16(duslo, 0)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(duslo, 1)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(duslo, 2)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(duslo, 3)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(duslo, 4)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(duslo, 5)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(duslo, 6)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(duslo, 7)] += m_prob;
          //if(k < (8-d))
          m_comatrix_data[_mm_extract_epi16(dushi, 0)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(dushi, 1)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(dushi, 2)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(dushi, 3)] += m_prob;
        }
        else
        {
          m_comatrix_data[_mm_extract_epi16(duslo, 0)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(dushi, 0)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(duslo, 1)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(dushi, 1)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(duslo, 2)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(dushi, 2)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(duslo, 3)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(dushi, 3)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(duslo, 4)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(dushi, 4)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(duslo, 5)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(dushi, 5)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(duslo, 6)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(dushi, 6)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(duslo, 7)] += m_prob;
          m_comatrix_data[_mm_extract_epi16(dushi, 7)] += m_prob;
        }
      }

      p_ref += 16;
      p_cmp += 16;
    }
  }

  cvTranspose(m_comatrix, m_comatrixt);
  cvAdd(m_comatrix, m_comatrixt, m_comatrix);
}

template <class T> void Texture<T>::ComputeHaralickFeatures(std::vector<float> &vTexture)
{
  float sumv = 0, difv = 0;
  memset(m_asum, 0, (2*m_graylevels-1)*sizeof(float));
  memset(m_adif, 0, m_graylevels*sizeof(float));

  std::vector<float> vTex(8);

  for (int i = 0; i < m_graylevels; ++i)
  {
    for (int j = 0; j < m_graylevels; ++j)
    {
      vTex[0] += pow(CV_IMAGE_ELEM(m_comatrix, float, i, j),2);  //Angular second momentum
      if(CV_IMAGE_ELEM(m_comatrix, float, i, j) != 0)
        vTex[1] += CV_IMAGE_ELEM(m_comatrix, float, i, j)*(-log(CV_IMAGE_ELEM(m_comatrix, float, i, j))); //Entropy
      vTex[2] += CV_IMAGE_ELEM(m_con, float, i, j)*CV_IMAGE_ELEM(m_comatrix, float, i, j); //Contrast
      vTex[3] += CV_IMAGE_ELEM(m_idm, float, i, j)*CV_IMAGE_ELEM(m_comatrix, float, i, j); //Inverse difference moment (homogeneity)
      sumv += CV_IMAGE_ELEM(m_sum, int, i, j)*CV_IMAGE_ELEM(m_comatrix, float, i, j); //sum average
      difv += CV_IMAGE_ELEM(m_dif, int, i, j)*CV_IMAGE_ELEM(m_comatrix, float, i, j); //difference average
      m_asum[CV_IMAGE_ELEM(m_sum, int, i, j)] += CV_IMAGE_ELEM(m_comatrix, float, i, j);
      m_adif[CV_IMAGE_ELEM(m_dif, int, i, j)] += CV_IMAGE_ELEM(m_comatrix, float, i, j);
    }
  }

  for (int k = 0; k < 2*m_graylevels; ++k)
  {
    vTex[4] += pow(k - sumv, 2)*m_asum[k]; //sum variance  
    if(m_asum[k] != 0)
      vTex[5] += m_asum[k]*-log(m_asum[k]); //sum entropy  
  }

  for (int k = 0; k < m_graylevels; ++k)
  {
    vTex[6] += pow(k - difv, 2)*m_adif[k]; //difference variance  
    if(m_adif[k] != 0)
      vTex[7] += m_adif[k]*-log(m_adif[k]); //difference entropy  
  }

  vTexture.insert(vTexture.end(), vTex.begin(), vTex.end());
}

template <class T> void Texture<T>::ComputeHaralickFeaturesSSE(std::vector<float> &vTexture)
{
  memset(m_asum, 0, (2*m_graylevels-1)*sizeof(float));
  memset(m_adif, 0, m_graylevels*sizeof(float));

  float *p_comatrix = &CV_IMAGE_ELEM(m_comatrix, float, 0, 0);
  float *p_idm = &CV_IMAGE_ELEM(m_idm, float, 0, 0);
  float *p_con = &CV_IMAGE_ELEM(m_con, float, 0, 0);
  int *p_sum = &CV_IMAGE_ELEM(m_sum, int, 0, 0);
  int *p_dif = &CV_IMAGE_ELEM(m_dif, int, 0, 0);
  float *p_asum = m_asum;
  float *p_adif = m_adif;

  __m128 asmv = _mm_setzero_ps();
  __m128 entv = _mm_setzero_ps();
  __m128 conv = _mm_setzero_ps();
  __m128 homv = _mm_setzero_ps();
  __m128 summ = _mm_setzero_ps();
  __m128 difm = _mm_setzero_ps();
  __m128 sumv = _mm_setzero_ps();
  __m128 difv = _mm_setzero_ps();
  __m128 sume = _mm_setzero_ps();
  __m128 dife = _mm_setzero_ps();

  for (int i = 0; i < m_graylevels*m_graylevels; i += 4)
  {
    __m128 cm = _mm_loadu_ps(p_comatrix);
    __m128 hw = _mm_loadu_ps(p_idm);
    __m128 cw = _mm_loadu_ps(p_con);
    __m128i sw = (__m128i)_mm_loadu_ps((const float*)p_sum);
    __m128i dw = (__m128i)_mm_loadu_ps((const float*)p_dif);

    asmv = _mm_add_ps(asmv, _mm_mul_ps(cm, cm));
    entv = _mm_add_ps(entv, _mm_mul_ps(cm, _mm_sub_ps(_mm_setzero_ps(), log_ps(cm))));
    conv = _mm_add_ps(conv, _mm_mul_ps(cw, cm));
    homv = _mm_add_ps(homv, _mm_mul_ps(hw, cm));
    summ = _mm_add_ps(summ, _mm_mul_ps(_mm_cvtepi32_ps(sw), cm));
    difm = _mm_add_ps(difm, _mm_mul_ps(_mm_cvtepi32_ps(dw), cm));

    m_asum[m_sum_data[i]]   += m_comatrix_data[i];
    m_asum[m_sum_data[i+1]] += m_comatrix_data[i+1];
    m_asum[m_sum_data[i+2]] += m_comatrix_data[i+2];
    m_asum[m_sum_data[i+3]] += m_comatrix_data[i+3];
    m_adif[m_dif_data[i]]   += m_comatrix_data[i];
    m_adif[m_dif_data[i+1]] += m_comatrix_data[i+1];
    m_adif[m_dif_data[i+2]] += m_comatrix_data[i+2];
    m_adif[m_dif_data[i+3]] += m_comatrix_data[i+3];

    p_comatrix += 4;
    p_idm += 4;
    p_con += 4;
    p_sum += 4;
    p_dif += 4;
  }

  summ = _mm_hadd_ps(summ, summ);
  summ = _mm_hadd_ps(summ, summ);
  difm = _mm_hadd_ps(difm, difm);
  difm = _mm_hadd_ps(difm, difm);

  for (int k = 0; k < 2*m_graylevels; k += 4)
  {
    __m128 as = _mm_loadu_ps(p_asum);
    __m128 f = _mm_sub_ps(_mm_set_ps(k+3, k+2, k+1, k), summ);
    sumv = _mm_add_ps(sumv, _mm_mul_ps(_mm_mul_ps(f, f), as));
    sume = _mm_add_ps(sume, _mm_mul_ps(as, _mm_sub_ps(_mm_setzero_ps(), log_ps(as))));

    p_asum += 4;
  }

  for (int k = 0; k < m_graylevels; k += 4)
  {
    __m128 ad = _mm_loadu_ps(p_adif);
    __m128 f = _mm_sub_ps(_mm_set_ps(k+3, k+2, k+1, k), difm);
    difv = _mm_add_ps(difv, _mm_mul_ps(_mm_mul_ps(f, f), ad));
    dife = _mm_add_ps(dife, _mm_mul_ps(ad, _mm_sub_ps(_mm_setzero_ps(), log_ps(ad))));

    p_adif += 4;
  }
  
  float haralick[8];
  _MM_TRANSPOSE4_PS(asmv, entv, conv, homv);
  _mm_storeu_ps(haralick, _mm_add_ps(_mm_add_ps(asmv,entv), _mm_add_ps(conv,homv)));
  _MM_TRANSPOSE4_PS(sumv, sume, difv, dife);
  _mm_storeu_ps(haralick+4, _mm_add_ps(_mm_add_ps(sumv,sume), _mm_add_ps(difv,dife)));

  for(int i = 0; i < 8; ++i)
    vTexture.push_back(haralick[i]);
  //vTexture.insert(vTexture.begin(), haralick, haralick+8);
}

template <class T> __m128 Texture<T>::log_ps(__m128 x) 
{
  static const __m128 _ps_1(_mm_set1_ps(1.0f));
  static const __m128 _ps_min_norm_pos((__m128)_mm_set1_epi32(0x00800000));
  static const __m128 _ps_inv_mant_mask((__m128)_mm_set1_epi32(~0x7f800000));
  static const __m128 _ps_0p5(_mm_set1_ps(0.5f));
  static const __m128i _pi32_0x7f(_mm_set1_epi32(0x7f));
  static const __m128 _ps_cephes_SQRTHF(_mm_set1_ps(0.707106781186547524));
  static const __m128 _ps_cephes_log_p0(_mm_set1_ps(7.0376836292E-2));
  static const __m128 _ps_cephes_log_p1(_mm_set1_ps(- 1.1514610310E-1));
  static const __m128 _ps_cephes_log_p2(_mm_set1_ps(1.1676998740E-1));
  static const __m128 _ps_cephes_log_p3(_mm_set1_ps(- 1.2420140846E-1));
  static const __m128 _ps_cephes_log_p4(_mm_set1_ps(+ 1.4249322787E-1));
  static const __m128 _ps_cephes_log_p5(_mm_set1_ps(- 1.6668057665E-1));
  static const __m128 _ps_cephes_log_p6(_mm_set1_ps(+ 2.0000714765E-1));
  static const __m128 _ps_cephes_log_p7(_mm_set1_ps(- 2.4999993993E-1));
  static const __m128 _ps_cephes_log_p8(_mm_set1_ps(+ 3.3333331174E-1));
  static const __m128 _ps_cephes_log_q1(_mm_set1_ps(-2.12194440e-4));
  static const __m128 _ps_cephes_log_q2(_mm_set1_ps(0.693359375));

  __m128i emm0;

  __m128 invalid_mask = _mm_cmple_ps(x, _mm_setzero_ps());

  x = _mm_max_ps(x, _ps_min_norm_pos);  /* cut off denormalized stuff */

  emm0 = _mm_srli_epi32(_mm_castps_si128(x), 23);

  /* keep only the fractional part */
  x = _mm_and_ps(x, _ps_inv_mant_mask);
  x = _mm_or_ps(x, _ps_0p5);

  emm0 = _mm_sub_epi32(emm0, _pi32_0x7f);
  __m128 e = _mm_cvtepi32_ps(emm0);

  e = _mm_add_ps(e, _ps_1);

  /* part2: 
     if( x < SQRTHF ) {
       e -= 1;
       x = x + x - 1.0;
     } else { x = x - 1.0; }
  */
  __m128 mask = _mm_cmplt_ps(x, _ps_cephes_SQRTHF);
  __m128 tmp = _mm_and_ps(x, mask);
  x = _mm_sub_ps(x, _ps_1);
  e = _mm_sub_ps(e, _mm_and_ps(_ps_1, mask));
  x = _mm_add_ps(x, tmp);

  __m128 z = _mm_mul_ps(x,x);

  __m128 y = _ps_cephes_log_p0;
  y = _mm_mul_ps(y, x);
  y = _mm_add_ps(y, _ps_cephes_log_p1);
  y = _mm_mul_ps(y, x);
  y = _mm_add_ps(y, _ps_cephes_log_p2);
  y = _mm_mul_ps(y, x);
  y = _mm_add_ps(y, _ps_cephes_log_p3);
  y = _mm_mul_ps(y, x);
  y = _mm_add_ps(y, _ps_cephes_log_p4);
  y = _mm_mul_ps(y, x);
  y = _mm_add_ps(y, _ps_cephes_log_p5);
  y = _mm_mul_ps(y, x);
  y = _mm_add_ps(y, _ps_cephes_log_p6);
  y = _mm_mul_ps(y, x);
  y = _mm_add_ps(y, _ps_cephes_log_p7);
  y = _mm_mul_ps(y, x);
  y = _mm_add_ps(y, _ps_cephes_log_p8);
  y = _mm_mul_ps(y, x);

  y = _mm_mul_ps(y, z);

  tmp = _mm_mul_ps(e, _ps_cephes_log_q1);
  y = _mm_add_ps(y, tmp);

  tmp = _mm_mul_ps(z, _ps_0p5);
  y = _mm_sub_ps(y, tmp);

  tmp = _mm_mul_ps(e, _ps_cephes_log_q2);
  x = _mm_add_ps(x, y);
  x = _mm_add_ps(x, tmp);
//  x = _mm_or_ps(x, invalid_mask); // negative arg will be NAN
  x = _mm_andnot_ps(invalid_mask, x); // x <= 0 will be 0

  return x;
}

template <class T> void Texture<T>::convertRGBtoScaledHSV(const IplImage* rgb)
{
  CvRect patch = cvGetImageROI(rgb);
  uchar *p_rgb = (uchar*)rgb->imageData + patch.y*rgb->widthStep;
  uchar *p_h = (uchar*)m_h->imageData;
  uchar *p_s = (uchar*)m_s->imageData;
  uchar *p_v = (uchar*)m_v->imageData;

  for( ; patch.height--; p_rgb += rgb->widthStep, p_h += m_h->widthStep, p_s += m_s->widthStep, p_v += m_v->widthStep)
  {
    for(int i = 0; i < patch.width; ++i)
    {
      const int j = i + patch.x;
      const int b = p_rgb[3*j+2], g = p_rgb[3*j+1], r = p_rgb[3*j];
      int h, s, v;
      int vmin, diff, sh, offset;

      if(b > g)
      {
        if(r > b)
        {
          v = r;
          vmin = g;
          sh = g - b;
          offset = 0;
        }
        else if(r > g)
        {
          v = b;
          vmin = g;
          sh = r - g;
          offset = 4;
        }
        else
        {
          v = b;
          vmin = r;
          sh = r - g;
          offset = 4;
        }
      }
      else
      {
        if(r > g)
        {
          v = r;
          vmin = b;
          sh = g - b;
          offset = 0;
        }
        else if(r > b)
        {
          v = g;
          vmin = b;
          sh = b - r;
          offset = 2;
        }
        else
        {
          v = g;
          vmin = r;
          sh = b - r;
          offset = 2;
        }
      }

      diff = v - vmin;
      s = diff * m_div_tableG[v] >> m_hsv_shift;
      h = sh + offset*diff;
      h = m_map_tableG[(((sh + offset*diff) * m_div_table255[diff] * 15 + (1 << (m_hsv_shift + 6))) >> (6 + m_hsv_shift)) + (h < 0 ? 360 : 0)];

      p_h[i] = (uchar)h;
      p_s[i] = (uchar)s;
      p_v[i] = (uchar)(v * m_div_tableG[255] >> m_hsv_shift);
    }
  }

  //cvSaveImage("rgb.bmp", rgb);
  //cvSaveImage("h.bmp", m_h);
  //cvSaveImage("s.bmp", m_s);
  //cvSaveImage("v.bmp", m_v);
}

template <class T> const IplImage* Texture<T>::convertRGBtoExG(const IplImage* rgb)
{
  for(int i = 0; i < rgb->height; ++i)
  {
    for(int j = 0; j < rgb->width; ++j)
    {
      int blue = (int)CV_IMAGE_ELEM(rgb, uchar, i, j*3);
      int green = (int)CV_IMAGE_ELEM(rgb, uchar, i, j*3+1);
      int red = (int)CV_IMAGE_ELEM(rgb, uchar, i, j*3+2);        

      if(green < red || green < blue || green > 240)
        CV_IMAGE_ELEM(m_tmp, uchar, i, j) = 0;
      else
      {
        uchar val  = (green - blue) + (green - red);
        if(val > m_threshold)
          CV_IMAGE_ELEM(m_tmp, uchar, i, j) = val;
        else
          CV_IMAGE_ELEM(m_tmp, uchar, i, j) = 0;
      }
    }
  }

  static int n = 0;
  std::string filename = "exg_" + boost::lexical_cast<std::string>(n++) + ".jpg";
  cvSaveImage(filename.c_str(), m_tmp);
  return m_tmp;
}

template <class T> const IplImage* Texture<T>::convertRGBtoExR(const IplImage* rgb)
{
  for(int i = 0; i < rgb->height; ++i)
  {
    for(int j = 0; j < rgb->width; ++j)
    {
      int blue = (int)CV_IMAGE_ELEM(rgb, uchar, i, j*3);
      int green = (int)CV_IMAGE_ELEM(rgb, uchar, i, j*3+1);
      int red = (int)(1.40 * CV_IMAGE_ELEM(rgb, uchar, i, j*3+2));

      if(red < (green + blue))
        CV_IMAGE_ELEM(m_tmp, uchar, i, j) = 0;
      else
        CV_IMAGE_ELEM(m_tmp, uchar, i, j) = std::min(red - (green+blue), 255);
    }
  }

  //static int n = 0;
  //std::string filename = "exr_" + boost::lexical_cast<std::string>(n++) + ".jpg";
  //cvSaveImage(filename.c_str(), m_tmp);
  return m_tmp;
}

template <class T> const IplImage* Texture<T>::convertRGBtoCIVE(const IplImage* rgb)
{
  for(int i = 0; i < rgb->height; ++i)
  {
    for(int j = 0; j < rgb->width; ++j)
    {
      int blue = (int)CV_IMAGE_ELEM(rgb, uchar, i, j*3);
      int green = (int)CV_IMAGE_ELEM(rgb, uchar, i, j*3+1);
      int red = (int)CV_IMAGE_ELEM(rgb, uchar, i, j*3+2);        

      CV_IMAGE_ELEM(m_tmp, uchar, i, j) = (uchar)(0.441*red - 0.811*green + 0.385*blue + 18.78745);
    }
  }

  //static int n = 0;
  //std::string filename = "cive_" + boost::lexical_cast<std::string>(n++) + "b.jpg";
  //SaveDoubleCvMat(filename.c_str(), m_tmp, false);
  return m_tmp;
}

template <class T> void Texture<T>::CreateGaborFilters(const CvSize size)
{
  m_gabor = new CvMat*[m_gabor_scales*m_gabor_orientations];
	int dft_M = cvGetOptimalDFTSize(2*size.height - 1);
  int dft_N = cvGetOptimalDFTSize(2*size.width - 1);
	CvMat* filter = cvCreateMat(dft_M, dft_N, CV_64FC2);
  int offset_x = size.width/2, offset_y = size.height/2;
  double sigmasq = m_gabor_sigma*m_gabor_sigma;

  for(int v = 0; v < m_gabor_scales; v++)
  {
    for(int u = 0; u < m_gabor_orientations; u++)
    {
		  double fv = pow(sqrt(2.0),-v)*m_gabor_fmax;
		  double phiu = u*M_PI/m_gabor_orientations;
		  double fvsq = fv*fv;

			cvZero(filter);
		  for(int y = 0; y < size.height; y++) 
      {
			  for(int x = 0; x < size.width; x++)
        {
				  int i = x - offset_x; 
				  int j = y - offset_y;
				  double mag = (double)(i*i+j*j);
				  ((double*)(filter->data.ptr + filter->step*y))[x*2] = fvsq/sigmasq * exp(-0.5*fvsq*mag/sigmasq) * (cos(fv*(i*cos(phiu)+j*sin(phiu)))-exp(-1.0*sigmasq/2.0));
				  ((double*)(filter->data.ptr + filter->step*y))[x*2+1] = fvsq/sigmasq * exp(-0.5*fvsq*mag/sigmasq) * (sin(fv*(i*cos(phiu)+j*sin(phiu))));
   			}
	    }

			//char coutfilename[512];
			//sprintf(coutfilename,"spatial%d_%d.bmp",v,u);
			//SaveDoubleCvMat(coutfilename, filter, true);

      m_gabor[v*m_gabor_orientations+u] = cvCreateMat( dft_M, dft_N, CV_64FC2);
 			cvDFT(filter, m_gabor[v*m_gabor_orientations+u], CV_DXT_FORWARD, size.height);

			//sprintf(coutfilename,"frequency%d_%d.bmp",v,u);
			//SaveDoubleCvMat(coutfilename, m_gabor[v*m_gabor_orientations+u], true);
    }
  }

	cvReleaseMat(&filter);
	fprintf(stdout," OK!\n");
}

template <class T> void Texture<T>::ReleaseGaborFilters()
{
  for(int i = 0; i < (m_gabor_scales*m_gabor_orientations); ++i)
    cvReleaseMat(&m_gabor[i]);
  delete [] m_gabor;
}


template <class T> void Texture<T>::SaveDoubleCvMat(const char* filename, CvArr* src, bool multichannel)
{
  IplImage tmp_header;
  CvSize size = cvGetSize(src);
  IplImage* ipl_src = cvGetImage(src, &tmp_header);
	IplImage* ipl_dst = cvCreateImage(size, IPL_DEPTH_8U, 1);

  IplImage* ipl_channel;
  if(multichannel)
  {
    ipl_channel = cvCreateImage(size, IPL_DEPTH_64F, 1);
    cvSplit(ipl_src, ipl_channel, NULL, NULL, NULL);
  }
  else
    ipl_channel = ipl_src;

	double maxVal,minVal;
	CvPoint minLoc, maxLoc;
	cvMinMaxLoc(ipl_channel, &minVal, &maxVal,&minLoc, &maxLoc, 0 );
	cvConvertScale(ipl_channel, ipl_dst, 255.0/(maxVal-minVal), -minVal*255.0/(maxVal-minVal));	
	cvSaveImage(filename,ipl_dst);

	cvReleaseImage(&ipl_dst);
  if(ipl_channel != ipl_src)
    cvReleaseImage(&ipl_channel);

  /*CvMat* pmat;
  if(multichannel)
  {
    pmat = cvCreateMat(mat->rows, mat->cols, CV_64FC1);
    cvSplit(mat, pmat, NULL, NULL, NULL);
  }
  else
    pmat = mat;

	double maxVal,minVal;
	CvPoint minLoc, maxLoc;
	cvMinMaxLoc(pmat, &minVal, &maxVal,&minLoc, &maxLoc, 0 );
  IplImage tmp_header;
	IplImage* tt = cvGetImage(pmat, &tmp_header);
	IplImage* ipl = cvCreateImage(cvGetSize(tt) ,8,1);
	cvConvertScale(tt,ipl,255.0/(maxVal-minVal),-minVal*255.0/(maxVal-minVal));	
	cvSaveImage(filename,ipl);
	cvReleaseImage(&ipl);

  if(mat != pmat)
    cvReleaseMat(&pmat);*/
}

template <class T> void Texture<T>::MultiplyFFT(const CvArr* srcAarr, const CvArr* srcBarr, CvArr* dstarr)
{
  CvMat *srcA = (CvMat*)srcAarr;
  CvMat *srcB = (CvMat*)srcBarr;
  CvMat *dst = (CvMat*)dstarr;

  for(int i = 0; i < srcA->rows; ++i)
  {
    for(int j = 0; j < srcA->cols; ++j)
    {
	    ((double*)(dst->data.ptr + dst->step*i))[j*2]	= ((double*)(srcA->data.ptr + srcA->step*i))[j*2]*((double*)(srcB->data.ptr + srcB->step*i))[j*2] -
	((double*)(srcA->data.ptr + srcA->step*i))[j*2+1]*((double*)(srcB->data.ptr + srcB->step*i))[j*2+1];
	    ((double*)(dst->data.ptr + dst->step*i))[j*2+1]	= ((double*)(srcA->data.ptr + srcA->step*i))[j*2]*((double*)(srcB->data.ptr + srcB->step*i))[j*2+1] +
((double*)(srcA->data.ptr + srcA->step*i))[j*2+1]*((double*)(srcB->data.ptr + srcB->step*i))[j*2];
    }
  }
}


#endif
